// Utilities for sensor measurements

int smooth(int currentData, int previousData, float smoothFactor) {
  return (previousData * (1 - smoothFactor) + (currentData * smoothFactor));
}

int findMode(int *data, int arraySize) {
  // The mode of a set of numbers is the value that occurs most frequently
  boolean done = 0;
  byte i;
  int temp, maxData, frequency, maxFrequency;
  
  // Sorts numbers from lowest to highest
  while (done != 1) {        
    done = 1;
    for (i=0; i<(arraySize-1); i++) {
      if (data[i] > data[i+1]) {     // numbers are out of order - swap
        temp = data[i+1];
        data[i+1] = data[i];
        data[i] = temp;
        done = 0;
      }
    }
  }
  
  temp = 0;
  frequency = 0;
  maxFrequency = 0;
  
  // Count number of times a value occurs in sorted array
  for (i=0; i<arraySize; i++) {
    if (data[i] > temp) {
      frequency = 0;
      temp = data[i];
      frequency++;
    } else if (data[i] == temp) frequency++;
    if (frequency > maxFrequency) {
      maxFrequency = frequency;
      maxData = data[i];
    }
  }
  return maxData;
}

// Allows user to zero gyros on command
void zeroGyros() {
  byte i;
  for (i=0; i<FINDZERO; i++) findZero[i] = analogRead(ROLLRATEPIN);
  rollGyroZero = findMode(findZero, FINDZERO);
  for (i=0; i<FINDZERO; i++) findZero[i] = analogRead(PITCHRATEPIN);
  pitchGyroZero = findMode(findZero, FINDZERO);
  for (i=0; i<FINDZERO; i++) findZero[i] = analogRead(YAWRATEPIN);
  yawGyroZero = findMode(findZero, FINDZERO);
}

// Allows user to zero accelerometers on command
void zeroAccelerometers() {
  byte i;
  for (i=0; i<FINDZERO; i++) findZero[i] = analogRead(ROLLACCELPIN);
  rollAccelZero = findMode(findZero, FINDZERO);
  for (i=0; i<FINDZERO; i++) findZero[i] = analogRead(PITCHACCELPIN);
  pitchAccelZero = findMode(findZero, FINDZERO);
}

// Works faster and is smaller than the constrain() function
int limitRange(int data, int minLimit, int maxLimit) {
  if (data < minLimit) return minLimit;
  else if (data > maxLimit) return maxLimit;
  else return data;
}

